#include "compsoc.h"
#include "memmap.h"

#define max(a, b) ((a)>(b))?(a):(b)
#define min(a, b) ((a)>(b))?(b):(a)

// Kernels for edge and blur filters
float Edge[] =
{
	 0, -1,  0,
	-1,  4, -1,
	 0, -1,  0
};

float Blur[] =
{
	0.0, 0.2,  0.0,
	0.2, 0.2,  0.2,
	0.0, 0.2,  0.0
};

const int filterWidth = 3;
const int filterHeight = 3;

//
#define imageWidth (240)
#define imageHeight (320)

#define rowSize (imageWidth * sizeof(pixel_t))
#define colSize (imageHeight * sizeof(pixel_t))

typedef struct {
	unsigned char R;
	unsigned char G;
	unsigned char B;
} pixel_t;

typedef struct {
	pixel_t Pixel[imageWidth];
} row_t;

// Local buffer
row_t bufferLocal[7];

//
void filterRow(pixel_t* preRow, pixel_t* row, pixel_t* postRow, pixel_t* destRow, int y) {

	pixel_t* rows[3] = {preRow, row, postRow};

	int x;
	for (x = 0; x < imageWidth; x++) {
	
		// Edge filter
		float edge = 0.0f;

		int filterX;
		for (filterX = -1; filterX <= +1; filterX++) {

			int filterY;
			for (filterY = 0; filterY < 3; filterY++) {
			
				pixel_t* filterRow = rows[filterY];

				pixel_t pixel = filterRow[(x + filterX + imageWidth) % imageWidth];

				float kernel = Edge[(filterX + 1) + filterY * 3];

				int pixelSum = pixel.R + pixel.G + pixel.B;

				edge += pixelSum * kernel;
			}
		}

		// Check if pixel is above edge treshold
		const float edgeTreshold = 61.2f; // 0.08f * 255 * 3

		// Filter
		float red = 0.0f;
		float green = 0.0f;
		float blue = 0.0f;

		float distanceToCenter = (sqrtf(powf((x - imageWidth / 2), 2) + powf((y - imageHeight / 2), 2)));

		float val = distanceToCenter / 255;
		float valr = (sqrtf(powf((x - imageWidth/2),2) + powf((y - imageHeight/2),2))) / 255;
		float valg = (sqrtf(powf((x - 50 - imageWidth/2),2) + powf((y + 75 - imageHeight/2),2))) / 255;
		float valb = (sqrtf(powf((x + 50 - imageWidth/2),2) + powf((y + 75 - imageHeight/2),2))) / 255;
		
		if (edge <= edgeTreshold) {

			// Blur filter
			int filterX;
			for (filterX = -1; filterX <= +1; filterX++) {
	
				int filterY;
				for (filterY = 0; filterY < 3; filterY++) {
			
					pixel_t* filterRow = rows[filterY];

					pixel_t pixel = filterRow[(x + filterX + imageWidth) % imageWidth];

					float kernel = Blur[(filterX + 1) + filterY * 3];

					red += pixel.R * kernel;
					green += pixel.G * kernel;
					blue += pixel.B * kernel;
				}
			}

			// Linear interpolate between source pixel and blurred pixel
			red   = row[x].R*(1-val) + val*red;
			green = row[x].G*(1-val) + val*green;
			blue  = row[x].B*(1-val) + val*blue;

		} else {

			red   = row[x].R;
			green = row[x].G;
			blue  = row[x].B;
		}

		// Scale and clamp to [0, 255]
		unsigned char R = (unsigned char) min(max((int)((1.3f + valr * 0.3f) * red + 0), 0), 255);
		unsigned char G = (unsigned char) min(max((int)((1.0f + valg * 0.3f) * green + 0), 0), 255);
		unsigned char B = (unsigned char) min(max((int)((0.7f + valb * 0.3f) * blue + 0), 0), 255);

		// Scale based on distance to center,
		// write the result to destRow
		float scaleDistance = (355 - distanceToCenter) / 255;

		destRow[x].R = (unsigned char) min(max((int)(scaleDistance*R), 0), 255);
		destRow[x].G = (unsigned char) min(max((int)(scaleDistance*G), 0), 255);
		destRow[x].B = (unsigned char) min(max((int)(scaleDistance*B), 0), 255);
	}

}

void copyRows(pixel_t* source, pixel_t* destination, int numberOfRows) {

	int x;
	for (x = 0; x < imageWidth * numberOfRows; x++) {
		destination[x] = source[x];
	}
}

void receiveRows(pixel_t* buffer, DMA* dma, int startRow, int numberOfRows) {

	hw_dma_receive((void*) buffer, (void*) frame_mem_pt + rowSize * startRow, (rowSize * numberOfRows) / 4, dma);
}

void sendRows(pixel_t* buffer, DMA* dma, int startRow, int numberOfRows) {

	hw_dma_send((void*) frame_mem_pt + rowSize * startRow, buffer, (rowSize * numberOfRows) / 4, dma);
}

void waitForFrameBuffer(int* buffer, DMA* dma) {

	*buffer	= 0;

	hw_dma_receive((void*) buffer, (void*) frame_mem_pt + rowSize * imageHeight - 4, 1, dma);

	while(*buffer == 0);
}

void waitSleep() {

	const unsigned int waitTime = 14000 * 8;

	unsigned int startTime = get_system_time();

	while (get_system_time() < startTime + waitTime);
}

int main() {


	int startTime = get_system_time();
	print_debug(1);
	print_debug(startTime);

	// Test Application application
	mon_sync(3);

	int afterSync = get_system_time();
	print_debug(2);
	print_debug(afterSync);

	// DMA0 buffer
	DMA dma0 = {1, (void*) dma0_BASEADDR};
	
	row_t* bufferDMA0 = (void*) cmemout0_BASEADDR;

	int i;
	for (i = 0; i < imageHeight; i += 5) {

		// Receive rows
		receiveRows(&bufferDMA0[0], &dma0, i, 5);

		if (i == 0) {
			// Duplicate first row
			copyRows(&bufferDMA0[0], &bufferLocal[1], 1);
		}


		// Copy rows
		copyRows(&bufferDMA0[0], &bufferLocal[2], 5);

		if (i == imageHeight / 2) {
			int halfway = get_system_time();
			print_debug(3);
			print_debug(halfway);
		}

		// Filter
		if (i > 0) {		
			filterRow(&bufferLocal[0], &bufferLocal[1], &bufferLocal[2], &bufferDMA0[0], i - 1);	
		}

		filterRow(&bufferLocal[1], &bufferLocal[2], &bufferLocal[3], &bufferDMA0[1], i + 0);
		filterRow(&bufferLocal[2], &bufferLocal[3], &bufferLocal[4], &bufferDMA0[2], i + 1);
		filterRow(&bufferLocal[3], &bufferLocal[4], &bufferLocal[5], &bufferDMA0[3], i + 2);
		filterRow(&bufferLocal[4], &bufferLocal[5], &bufferLocal[6], &bufferDMA0[4], i + 3);
		
		// Copy rows
		copyRows(&bufferLocal[5], &bufferLocal[0], 2);

		// Send rows
		if (i == 0) {
			sendRows(&bufferDMA0[1], &dma0, i, 4);
		} else {
			sendRows(&bufferDMA0[0], &dma0, i - 1, 5);
		}
	}

	// Filter last row
	filterRow(&bufferLocal[5], &bufferLocal[6], &bufferLocal[6], &bufferDMA0[0], 319);

	// Send last row
	sendRows(&bufferDMA0[0], &dma0, 319, 1);

	int afterProcessing = get_system_time();
	print_debug(4);
	print_debug(afterProcessing);

	// Wait until all data has been send to the frame buffer
	waitForFrameBuffer((int*) &bufferDMA0[1], &dma0);
	// waitSleep();

	int afterWait = get_system_time();
	print_debug(5);
	print_debug(afterWait);


	// Print result
	print_framebuffer();

	int afterPrint = get_system_time();
	print_debug(6);
	print_debug(afterPrint);

	return 0;
}
